Improving Path Execution in Deformable Environments Using Reactive Cost-space Control

We present a reactive controller designed to improve the execution of cost-space paths in the presence of sensor and modeling error that would otherwise result in significantly increased cost. In particular, we are concerned with the execution of paths planned in deformable environments, such as those produced using our cost-space planners [1]. In our previous work, executions of paths produced by these planners have suffered from significant unexpected object deformation. Execution of planned paths in cost spaces is complicated by errors and inaccuracy in the robot’s sensors, uncertainty in modeling the environment, error in the robot’s controllers, and fundamental limitations of the motion planner. When executed, these paths can incur significantly higher cost than predicted by the planner’s cost function. Modeling and sensor limitations are particularly pronounced with deformable environments, as the available models for deformable objects trade accuracy for reduced computational complexity [2], [3], [1]. To produce motion plans in acceptable time limits, existing planners use lower resolution or simplified models such as the voxel-based model introduced in our previous work [1]. Because these simplified models are inherently inaccurate, planners using them will fail to fully capture the behavior of the deformable objects. Similar issues are faced with rigid environments when using multiple-resolution models [4], [5], however, in these cases an accurate high-resolution model can be used inside the planning process [5] or for post-processing such as [6], [7] using trajectory optimizers such as AICO [8], CHOMP [9], or STOMP [10]. Such an approach can compensate for planning with a reduced resolution to improve planning time. However, since an accurate model of a deformable environment may not be available (indeed, it may be infeasible to simulate the deformable behavior), it may not be practical (or possible) to optimize paths in deformable environments before execution. Another challenge arises from the object deformations themselves. Since manipulation with deformable objects often involves actively changing the environment itself, we are often limited in the range of useful sensor information. For example, depth cameras may provide an accurate view of the undeformed obstacles ahead, but they cannot capture how the obstacles will deform when in contact. In contrast, touch or pressure sensors can provide a much better estimate of the deformation of the environment around the robot, but can only provide information on the current state. This is similar to the problem that occurs with operating in the presence of moving obstacles, where an accurate model of the entire environment is not available prior to execution. In cases with rigid objects, this problem has been addressed in a variety of ways, such as [6], [7], [11], [12]; provided accurate information on the moving objects is available, the path can be deformed online to increase clearance. Alternatively, reactive control can be combined into the motion planning process a priori, such as [13], effectively anticipating changes during execution. In contrast, because of the aforementioned sensor limitations when using deformable objects, accurate information on the deformed environment may only be available at very short range or while in contact with obstacles, preventing any broader adaptation or optimization of the path. For robots operating in rigid environments with similar limits on sensor horizon, several planning and control schemes have been proposed [14], [15]. Our proposed reactive controller modifies a cost-space path on-the-fly during execution in an attempt to locally optimize the planned path to the real environment. While trajectory optimization techniques have already been applied to motion planning with deformable objects [2], our approach differs by performing the optimization incrementally online in a manner similar to path deformation [6], [7], [11], [12]. This approach combines the main strengths of motion planning, namely the completeness properties and global scope of the motion planner producing the initial path, with the reactive strengths of local control. Using the planner to provide a “guiding path” for local control addresses wellknown problems of local control such as becoming stuck in local minima and failing to reach the goal. Complementing this, the local controller addresses one of the inherent limitations of most motion planning the inability to react to unexpected changes or differences in the environment without expensive re-planning.